#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTIRS2,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorLEFT,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorRIGHT,    tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/HTIRS2-driver.h"
int _dirAC = 0;
int _Dir;
int acS1, acS2, acS3, acS4, acS5 = 0;
tHTIRS2DSPMode _mode = DSP_1200;
int DirPwr(int Dir);
int SelectDir();
task main
{
  //_Dir = SelectDir();
_Dir = 5;
  PlaySoundFile("! Click.rso");
  // read the current modulated signal direction
  _dirAC = HTIRS2readACDir(HTIRS2);
  // I2C read error occurred
  HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
  if(_Dir < _dirAC)
  {
    while(_dirAC != _Dir)
    {
      motor[motorLEFT] = 100;
      motor[motorRIGHT] = -100;
     // wait10Msec(70);
      _dirAC = HTIRS2readACDir(HTIRS2);
    }
  }
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
  if(_Dir > _dirAC)
  {
    while(_dirAC != _Dir)
    {
      motor[motorLEFT] = -100;
      motor[motorRIGHT] = 100;
      _dirAC = HTIRS2readACDir(HTIRS2);
    }
  }
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
  while(DirPwr(_Dir) < 60)
  {
    motor[motorLEFT] = 100;
    motor[motorRIGHT] = 100;
  }
  motor[motorLEFT] = 0;
  motor[motorRIGHT] = 0;
}
int DirPwr(int Dir)
{
  HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
  switch(Dir)
  {
  case 1:
    return acS1;
    break;
  case 2:
    return ((acS1 + acS2)/2);
    break;
  case 3:
    return acS2;
    break;
  case 4:
    return ((acS2 + acS3)/2);
    break;
  case 5:
    return acS3;
    break;
  case 6:
    return ((acS3 + acS4)/2);
    break;
  case 7:
    return acS4;
    break;
  case 8:
    return ((acS4 + acS5)/2);
    break;
  case 9:
    return acS5;
    break;
  default:
    return 0;
    break;
  }
}
int SelectDir()
{
  int nIndex = 1;
  while(true)
  {
    if(nNxtButtonPressed == kLeftButton)
    {
      if(nIndex == 1)
        nIndex = 9;
      else
        nIndex--;
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
    }
    if(nNxtButtonPressed == kRightButton)
    {
      if(nIndex == 9)
        nIndex = 1;
      else
        nIndex++;
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
    }
    if(nNxtButtonPressed == kEnterButton)
    {
      wait1Msec(300);
      PlaySoundFile("! Click.rso");
      break;
    }
    nxtDisplayStringAt(0, 31, "Direction?");
    nxtDisplayBigStringAt(0, 15, "%d", nIndex);
    wait1Msec(100);
  }
  eraseDisplay();
  nxtDisplayStringAt(0, 15, "Searching...");
  return(nIndex);
}
